#include "Core.h"

stCoreTimer *pTimerTop;

void Core_SetTimer(stCoreTimer *pTimer)
{
    stCoreTimer *pt = pTimerTop;
    stCoreTimer *ppt = NULL;

    CORE_TIMER_STOP;
    LOCK_INTERRUPT;

    if (pTimerTop == NULL) {
        pTimer->pNext = pTimerTop;
        pTimerTop = pTimer;
    }

    while (pt != NULL) {
        pt->Count -= CORE_TIMER_VAL;
        if (pt->Count > pTimer->Count) {
            pTimer->pNext = pt;
            if (ppt != NULL) {
                ppt->pNext = pTimer;
            } else {
                pTimerTop = pTimer;
            }
        } else {
            if (pt->pNext == NULL) {
                pTimer->pNext = pt->pNext;
                pt->pNext = pTimer;
                break;
            }
        }
        ppt = pt;
        pt = pt->pNext;
    }

    UNLOCK_INTERRUPT;
    Core_StartTimer();
}

void Core_RunTimer(UInt16 Count)
{
    stCoreTimer *pt = pTimerTop;
    stCoreTimer *ppt = NULL;
    stCoreTimer *ptTimeout = NULL;

    LOCK_INTERRUPT;
    while (pt != NULL) {
        pt->Count -= Count;

        if (pt->Count < 50) {
            stCoreTimer *pTmp = pt->pNext;
            if (pt == pTimerTop) {
                pTimerTop = pTmp;
            } else {
                ppt->pNext = pTmp;
            }

            pt->pNext = ptTimeout;
            ptTimeout = pt;

            pt = pTmp;
        } else {
            ppt = pt;
            pt = pt->pNext;
        }
    }
    UNLOCK_INTERRUPT;

    pt = ptTimeout;
    while (pt != NULL) {
        ptTimeout = pt->pNext;
        pt->OnTimeOut(pt);
        pt = ptTimeout;
    }
}

void Core_StartTimer(void)
{
    // SET PHY TIMER
    //CORE_TIMER_VAL = 0;
    CORE_TIMER_MOD = pTimerTop->Count;
    CORE_TIMER_START;
}

void Core_PhyTimerInterrupt(void)
{
    Core_RunTimer(pTimerTop->Count);
    if (pTimerTop != NULL) {
        Core_StartTimer();
    } else {
        CORE_TIMER_STOP;
    }
}
